#ifndef __MAP_BASE_H_
#define __MAP_BASE_H_


#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <mc_core/common/core.h>
#include <opencv2/opencv.hpp>


// using namespace cv;
using namespace std;

using OccupancyGrid = nav_msgs::OccupancyGrid;
using OccupancyGridPtr = nav_msgs::OccupancyGrid::Ptr;

class MapBase{
    
    public:
        MapBase(){};
    
        // 设置地图数据
        void setOccupancyGrid(const OccupancyGridPtr& occupancy_grid_ptr);

        // 世界坐标转像素坐标
        template <typename T1, typename T2>
        T2 WorldToPixel(const T1& point){
            T2 pixel_point(
                std::round((point.x - origin_.x) / resolution_), 
                std::round((point.y - origin_.y) / resolution_));
            return pixel_point;
        };

        // 世界坐标转像素坐标
        template <typename T1,  typename T2>
        T2 WorldToOpencvPixel(const T1& point){
            T2 pixel_point(
                std::round((point.x - origin_.x) / resolution_), 
                std::round((point.y - origin_.y) / resolution_));
            pixel_point.y = height_ - 1 - pixel_point.y; 
            return pixel_point;
        }

        // 像素坐标转世界坐标
        template <typename T1,  typename T2>
        T2 OpencvPixelToWorld(const T1& point){
            T2 world_point(
                origin_.x + (point.x + 0.5) * resolution_, 
                origin_.y + (height_ - 1 - point.y + 0.5) * resolution_
            );
            return world_point;
        };

        // 像素坐标转世界坐标
        template <typename T1,  typename T2>
        T2 PixelToWorld(const T1& point){
            T2 world_point(
                origin_.x + (point.x + 0.5) * resolution_, 
                origin_.y + (point.y + 0.5) * resolution_
            );
            return world_point;
        }

        // 二维数据转一维
        template <typename T>
        size_t ToIndex(T pixel_2d){
            return pixel_2d.y * width_ + pixel_2d.x;
        }

        // 是否有效
        template <typename T>
        bool IsValid(T pixel_2d){
            if(pixel_2d.x < 0 || pixel_2d.x >= width_)
                return false;
            if(pixel_2d.y < 0 || pixel_2d.y >= height_)
                return false;
            return true;
        };
        // bool IsValid(size_t idx);

        // 获取像素值，包含边界检测
        template <typename T>
        bool GetValue(T pixel_2d, int& value){
            if(IsValid(pixel_2d)){
                // value = cvMap_.at<uchar>(cv::Point2i(pixel_2d.x, pixel_2d.y));
                value = cvMap_.ptr<uchar>(pixel_2d.y)[pixel_2d.x];
                return true;
            }
            return false;
        }
        
        // 数值是否有效
        template <typename T>
        bool ValueIsValid(T pixel_2d){
            int value;
            if(GetValue(pixel_2d, value)){
                // cout << "value: " << value << endl;
                if(value < 200) 
                    return false;
                else
                    return true;
            }
            return false;
        }

        // 简单膨胀处理
        void SimpleDilateOccupancyGrid(double robot_radius);
        void SimpleDilateOpencvMap(double robot_radius);

        // 获取地图数据
        bool getMap(cv::Mat& cv_map);
        // 获取二值地图数据
        bool getMapBinary(cv::Mat& cv_map);
        bool getMap(nav_msgs::OccupancyGrid::Ptr& map_ptr, string frame_id);
        OccupancyGridPtr getOccupancyGrid(){
            return mapOriginGridPtr_;
        }
    
        // 获取基础数据
        int getWidth() const {return width_;}
        int getHeight() const {return height_;}
        cv::Point2f getOrigin() const {return origin_;}
        double getResolution() const {return resolution_;}
        string getFrameId() const {return frameId_;}

    protected:
        OccupancyGridPtr mapOriginGridPtr_;  // 地图数据
        cv::Mat cvMap_;         // 地图数据
        cv::Mat cvMapBinary_;         // 二值地图数据
        double resolution_;      // 地图分辨率
        int width_;             // 地图宽度
        int height_;            // 地图高度
        cv::Point2f origin_;          // 地图原点
        size_t mapSize_;       // 地图大小
        string frameId_;
};

#endif